#include "stm32f4xx.h"                  // Device header
#include "stm32f4xx_conf.h"
#include "GM6020.h"
#include "Delay.h"

#define GM6020_Speed			60		//转速

#define GM6020_PWMMode			0		//PWM模式(0-速度模式,1-位置模式)
#define GM6020_PWMChecksum		50		//GM6020校验值,对应1000us
#define GM6020_PWMTime			100		//校验完成等待时间

#define GM6020_Control_ID_L		0x1FF//GM6020低位ID发送报文标识符(GM6020低位标识符和M3508高位标识符相同)
#define GM6020_Control_ID_H		0x2FF//GM6020低位ID发送报文标识符

int16_t GM6020_PWMNowSpeedDuty=0;//PWM当前速度占空比
GM6020_Motor GM6020_MotorStatus[7];//M3508电机状态数组

/*
 *函数简介:PWM控制GM6020电机初始化
 *参数说明:无
 *返回类型:无
 *备注:默认使用PWM1(C1为PE9),默认50Hz
 *备注:需要校准"电调校验值"和"校验完成等待时间"
 */
void GM6020_PWMInit(void)
{
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1,ENABLE);
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOE,ENABLE);//开启时钟
	
	TIM_InternalClockConfig(TIM1);//选择时基单元TIM1
	
	GPIO_InitTypeDef GPIO_InitStructure;
	GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF;   
	GPIO_InitStructure.GPIO_OType=GPIO_OType_PP;//复用推挽
	GPIO_InitStructure.GPIO_PuPd=GPIO_PuPd_UP;//默认上拉
	GPIO_InitStructure.GPIO_Pin=GPIO_Pin_9;
	GPIO_InitStructure.GPIO_Speed=GPIO_Speed_100MHz;
	GPIO_Init(GPIOE,&GPIO_InitStructure);//配置C1-PE9
	
	GPIO_PinAFConfig(GPIOE,GPIO_PinSource9,GPIO_AF_TIM1);//开启C1的TIM1复用模式
	
	TIM_TimeBaseInitTypeDef TIM_InitStructure;
	TIM_InitStructure.TIM_ClockDivision=TIM_CKD_DIV1;//配置时钟分频为1分频
	TIM_InitStructure.TIM_CounterMode=TIM_CounterMode_Up;//配置计数器模式为向上计数
	TIM_InitStructure.TIM_Period=1000-1;//ARR,PWM为千分位20ms
	TIM_InitStructure.TIM_Prescaler=3360-1;//PSC
	TIM_InitStructure.TIM_RepetitionCounter=0;//配置重复计数单元的置为0
	TIM_TimeBaseInit(TIM1,&TIM_InitStructure);
	
	TIM_OCInitTypeDef TIM_OCInitStructure;
	TIM_OCStructInit(&TIM_OCInitStructure);
	TIM_OCInitStructure.TIM_OCMode=TIM_OCMode_PWM1;//配置输出比较模式
	TIM_OCInitStructure.TIM_OCPolarity=TIM_OCPolarity_High;//配置输出比较的极性
	TIM_OCInitStructure.TIM_OutputState=TIM_OutputState_Enable;//输出使能
	TIM_OCInitStructure.TIM_Pulse=0;//配置输出比较寄存器CCR的值
	TIM_OC1Init(TIM1,&TIM_OCInitStructure);//配置C1输出PWM
	
	TIM_Cmd(TIM1,ENABLE);//启动定时器
	TIM_CtrlPWMOutputs(TIM1,ENABLE);//开启TIM1的PWM输出
	
	TIM_SetCompare1(TIM1,GM6020_PWMChecksum);//电调校准
	Delay_ms(GM6020_PWMTime);//等待校准完成
	GM6020_PWMNowSpeedDuty=GM6020_PWMChecksum+25;//校验当前占空比
}

/*
 *函数简介:PWM设置GM6020转速
 *参数说明:速度-vm~+vm
 *参数说明:标志位,决定是否使用缓启动
 *返回类型:无
 *备注:仅用于GM6020速度模式
 */
void GM6020_PWMSetSpeed(int16_t Speed,uint8_t Flag)
{
	uint16_t Duty=0;
	if(Speed<0)//反转(0-1500,1080->v=-vm,1480->v=0)
	{
		Duty=GM6020_PWMChecksum+24+Speed*400/GM6020_Speed/20;//获取占空比
		if(Flag==0)//硬启动
			TIM_SetCompare1(TIM1,Duty);
		else//缓启动,10分位
			for(uint8_t i=0;i<=10;i++)
			{
				TIM_SetCompare1(TIM1,(int)(GM6020_PWMNowSpeedDuty+(Duty-GM6020_PWMNowSpeedDuty)*i/10));
				Delay_ms(20);
			}
	}
	else//正转(1500-2000,1520->v=0,1920->v=vm)
	{
		Duty=GM6020_PWMChecksum+26+Speed*400/GM6020_Speed/20;
		if(Flag==0)//硬启动
			TIM_SetCompare1(TIM1,Duty);
		else//从当前速度缓启动到目标速度,10分位
			for(uint8_t i=0;i<=10;i++)
			{
				TIM_SetCompare1(TIM1,(int)(GM6020_PWMNowSpeedDuty+(Duty-GM6020_PWMNowSpeedDuty)*i/10));
				Delay_ms(20);
			}
	}
	GM6020_PWMNowSpeedDuty=Duty;//获取当前占空比
}

/*
 *函数简介:PWM设置GM6020角度
 *参数说明:角度0~θm(默认360°)
 *返回类型:无
 *备注:仅用于GM6020位置模式
 */
void GM6020_PWMSetAngle(uint16_t Angle)
{
	uint16_t Duty=0;
	Duty=GM6020_PWMChecksum+4+Angle*42/360;//获取占空比
	TIM_SetCompare1(TIM1,Duty);
}

/*
 *函数简介:CAN总线设置GM6020低位ID电压
 *参数说明:Voltage1~4分别对应ID1~4
 *返回类型:1-发送成功,0-发送失败
 *备注:只能配置ID1~4(标识符0x1FF)
 *备注:默认标准格式数据帧,4字节数据段
 *备注:注意GM6020的ID1~4报文标识符与M3508的ID5~8报文标识符相同
 *备注:给电机一定的电压,会促使电机产生速度
 */
uint8_t GM6020_CANSetLIDVoltage(int16_t Voltage1,int16_t Voltage2,int16_t Voltage3,int16_t Voltage4)
{
	CanTxMsg TxMessage;
	TxMessage.StdId=GM6020_Control_ID_L;//低位ID标准标识符0x1FF
	TxMessage.RTR=CAN_RTR_Data;//数据帧
	TxMessage.IDE=CAN_Id_Standard;//标准格式
	TxMessage.DLC=0x08;//4字节数据段
	TxMessage.Data[0]=Voltage1>>8;//ID1电压高八位
	TxMessage.Data[1]=Voltage1;//ID1电压低八位
	TxMessage.Data[2]=Voltage2>>8;//ID2电压高八位
	TxMessage.Data[3]=Voltage2;//ID2电压低八位
	TxMessage.Data[4]=Voltage3>>8;//ID3电压高八位
	TxMessage.Data[5]=Voltage3;//ID3电压低八位
	TxMessage.Data[6]=Voltage4>>8;//ID4电压高八位
	TxMessage.Data[7]=Voltage4;//ID4电压低八位
	
	uint8_t mbox=CAN_Transmit(CAN1,&TxMessage);//发送数据并获取邮箱号
	uint16_t i=0;
	while((CAN_TransmitStatus(CAN1,mbox)==CAN_TxStatus_Failed)&&(i<0XFFF))i++;//等待发送结束
	if(i>=0xFFF)return 0;//发送失败
	return 1;//发送成功
}

/*
 *函数简介:CAN总线设置GM6020高位ID电压
 *参数说明:Voltage5~7分别对应ID5~7
 *返回类型:1-发送成功,0-发送失败
 *备注:只能配置ID5~7(标识符0x2FF)
 *备注:默认标准格式数据帧,4字节数据段
 *备注:给电机一定的电压,会促使电机产生速度
 */
uint8_t GM6020_CANSetHIDVoltage(int16_t Voltage5,int16_t Voltage6,int16_t Voltage7)
{
	CanTxMsg TxMessage;
	TxMessage.StdId=GM6020_Control_ID_H;//高位ID标准标识符0x2FF
	TxMessage.RTR=CAN_RTR_Data;//数据帧
	TxMessage.IDE=CAN_Id_Standard;//标准格式
	TxMessage.DLC=0x08;//4字节数据段
	TxMessage.Data[0]=Voltage5>>8;//ID5电压高八位
	TxMessage.Data[1]=Voltage5;//ID5电压低八位
	TxMessage.Data[2]=Voltage6>>8;//ID6电压高八位
	TxMessage.Data[3]=Voltage6;//ID6电压低八位
	TxMessage.Data[4]=Voltage7>>8;//ID7电压高八位
	TxMessage.Data[5]=Voltage7;//ID7电压低八位
	TxMessage.Data[6]=0;//空位,默认给0
	TxMessage.Data[7]=0;//空位,默认给0
	
	uint8_t mbox=CAN_Transmit(CAN1,&TxMessage);//发送数据并获取邮箱号
	uint16_t i=0;
	while((CAN_TransmitStatus(CAN1,mbox)==CAN_TxStatus_Failed)&&(i<0XFFF))i++;//等待发送结束
	if(i>=0xFFF)return 0;//发送失败
	return 1;//发送成功
}

/*
 *函数简介:GM6020数据处理
 *参数说明:GM6020电机ID号枚举,GM6020_1~7对应ID号0x205~0x20B
 *参数说明:反馈数据(8字节)
 *返回类型:无
 *备注:保存到GM6020_MotorStatus结构体数组
 */
void GM6020_CANDataProcess(GM6020_ID ID,uint8_t *Data)
{
	GM6020_MotorStatus[ID-0x205].Angle=(uint16_t)((((uint16_t)Data[0])<<8)|Data[1]);//机械角度
	GM6020_MotorStatus[ID-0x205].Speed=(int16_t)((((uint16_t)Data[2])<<8)|Data[3]);//转速
	GM6020_MotorStatus[ID-0x205].Current=(int16_t)((((uint16_t)Data[4])<<8)|Data[5]);//实际转矩电流
	GM6020_MotorStatus[ID-0x205].Temperature=Data[6];//电机温度
}
